
#include "Smoother.h"
#include <stdio.h>

Vertex::Vertex()
{

}

Vertex::~Vertex()
{

}

Triangle::Triangle()
{

}

Triangle::~Triangle()
{

}

void Smoother::ApplySmoothing( int iter )
{
	printf("%d",iter);

	for (int i=0;i<iter;i++)
	{
		SmootingIter();
		DataFlip();
	}	

}

ExplicitSmoother::ExplicitSmoother( float lambda_, float timeStep_ ) : lambda(lambda_), dt(timeStep_)
{
	vertices = new VertexList;
	newVertices = new VertexList;
}

ExplicitSmoother::~ExplicitSmoother( void )
{

}

void ExplicitSmoother::SmootingIter()
{
	for (int i=0;i<vertices->size();i++)
	{
		Vertex *nV, *oV;
		vector3 laplacian(0,0,0);

		nV=newVertices->at(i);
		oV=vertices->at(i);

		IndexList neighbours=getNeighbours(i);

		for (int j=0;j<neighbours.size();j++)
		{
			Vertex *nB=vertices->at(neighbours.at(j));

			laplacian+=nB->position-oV->position;
		}

		laplacian*=(lambda*dt/neighbours.size());

		nV->position=oV->position+laplacian;

		//nV->position=oV->position;
	}
}

void Smoother::DataFlip()
{
	for (int i=0;i<newVertices->size();i++)
	{
		Vertex *v=newVertices->at(i);

		vector3 normal(0,0,0);

		for (int j=0;j<v->neighbour.size();j++)
		{
			Triangle *tri=v->neighbour.at(j);

			normal+=getNormal(tri,v->index);
		}

		v->normal=normal.normalize();
	}

	VertexList* temp;

	temp=newVertices;
	newVertices=vertices;
	vertices=temp;
}

vector3 Smoother::getNormal( Triangle* tri, int v )
{
	Vertex *va, *vb, *vc;
	vector3 normal;

	va=newVertices->at(tri->v1);
	vb=newVertices->at(tri->v2);
	vc=newVertices->at(tri->v3);

	if (v==tri->v1)
	{
		normal=CrossProduct(vb->position-va->position, vc->position-va->position);
	}else if (v==tri->v2)
	{
		normal=CrossProduct(vc->position-vb->position, va->position-vb->position);
	}else if (v==tri->v3)
	{
		normal=CrossProduct(va->position-vc->position, vb->position-vc->position);
	}

	return normal.normalize();
}

IndexList Smoother::getNeighbours( int v )
{
	Vertex *vertex=vertices->at(v);
	IndexList vertexList; 

	for (int i=0;i<vertex->neighbour.size();i++)
	{
		Triangle *tri=vertex->neighbour.at(i);		

		if (tri->v1!=v)
		{
			bool duplicate=false;

			for (int j=0;j<vertexList.size();j++)
			{
				if (tri->v1==vertexList.at(j))
				{
					duplicate=true;
					break;
				}
			}

			if (!duplicate)
			{
				vertexList.push_back(tri->v1);
			}
		}

		if (tri->v2!=v)
		{
			bool duplicate=false;

			for (int j=0;j<vertexList.size();j++)
			{
				if (tri->v2==vertexList.at(j))
				{
					duplicate=true;
					break;
				}
			}

			if (!duplicate)
			{
				vertexList.push_back(tri->v2);
			}
		}

		if (tri->v3!=v)
		{
			bool duplicate=false;

			for (int j=0;j<vertexList.size();j++)
			{
				if (tri->v3==vertexList.at(j))
				{
					duplicate=true;
					break;
				}
			}

			if (!duplicate)
			{
				vertexList.push_back(tri->v3);
			}
		}

	}

	return vertexList;
}

ImplicitSmoother::ImplicitSmoother( float lambda_, float timeStep_ ) : lambda(lambda_), dt(timeStep_)
{
	vertices = new VertexList;
	newVertices = new VertexList;
}

ImplicitSmoother::~ImplicitSmoother( void )
{

}

void ImplicitSmoother::SmootingIter()
{
	InitSystem();
	SparseSolver();
	UpdateSolution();
}

void ImplicitSmoother::InitLaplacian()
{
	int scale=3*vertices->size();

	Laplacian=(float**)malloc(scale*sizeof(float*));
	index=(int**)malloc(scale*sizeof(int*));
	A=Laplacian;
	b=(float*)malloc(3*vertices->size()*sizeof(float));
	buffer=(float*)malloc(3*vertices->size()*sizeof(float));
	x=(float*)malloc(3*vertices->size()*sizeof(float));

	float multiplier=-1*lambda*dt;

	

	for (int i=0;i<scale;i++)
	{
		Laplacian[i]=(float*)malloc(3*RING*sizeof(float));
		index[i]=(int*)malloc(3*RING*sizeof(int));

		if (!Laplacian[i])
		{
			printf("fuck");
		}

		for (int j=0;j<3*RING;j++)
		{
			Laplacian[i][j]=0;
			index[i][j]=-1;
		}
	}

	for (int i=0;i<vertices->size();i++)
	{
		Vertex *vert;

		vert=vertices->at(i);

		int offset=0;

		Laplacian[i*3+0][offset]=-1*multiplier+1;
		Laplacian[i*3+1][offset]=-1*multiplier+1;
		Laplacian[i*3+2][offset]=-1*multiplier+1;

		index[i*3+0][offset]=3*i+0;
		index[i*3+1][offset]=3*i+1;
		index[i*3+2][offset]=3*i+2;

		offset++;		

		IndexList neighbours=getNeighbours(i);

		float coef=1.0/(float)neighbours.size();

		for (int j=0;j<neighbours.size();j++)
		{
			Vertex *nB=vertices->at(neighbours.at(j));

			Laplacian[i*3+0][offset]=coef*multiplier;
			Laplacian[i*3+1][offset]=coef*multiplier;
			Laplacian[i*3+2][offset]=coef*multiplier;

			index[i*3+0][offset]=3*neighbours.at(j)+0;
			index[i*3+1][offset]=3*neighbours.at(j)+1;
			index[i*3+2][offset]=3*neighbours.at(j)+2;

			offset++;
		}
	}

}

void ImplicitSmoother::InitSystem()
{
	for (int i=0;i<vertices->size();i++)
	{
		b[i*3+0]=vertices->at(i)->position.x;
		b[i*3+1]=vertices->at(i)->position.y;
		b[i*3+2]=vertices->at(i)->position.z;
	}

}

void ImplicitSmoother::SparseSolver()
{
	JacobiIter();
}

void ImplicitSmoother::JacobiIter()
{
	int scale=3*vertices->size();

	for (int loop=0;loop<1000;loop++)
	{
		for (int i=0;i<scale;i++)
		{
			float sum=0;

			for (int j=1;j<3*RING;j++)
			{
				if (index[i][j]==-1)
				{
					break;
				}

				sum+=A[i][j]*x[index[i][j]];
			}

			if (A[i][0]!=0)
			{
				buffer[i]=(b[i]-sum)/A[i][0];
			}

		}

		float residualMax=0;

		for (int i=0;i<scale;i++)
		{
			if (residualMax<abs(buffer[i]-x[i]))
			{
				residualMax=abs(buffer[i]-x[i]);
			}
			x[i]=buffer[i];
		}

		if (residualMax<TOLERATE)
		{
			break;
		}
	}

}

void ImplicitSmoother::UpdateSolution()
{
	for (int i=0;i<vertices->size();i++)
	{
		Vertex *nV=newVertices->at(i);

		nV->position=vector3(x[i*3+0],x[i*3+1],x[i*3+2]);
	}

}